module ghlliks
implicit none

contains
subroutine ghllik(m,th,likresm)
use datahadler
use linear_operators
use matrixfuns
use bessels
use ghs
integer, intent(in):: m
double precision, intent(in):: th(m)
double precision, intent(out):: likresm
integer T,n,i,j
integer,parameter:: fil=1000,col=3
double precision y(fil,col)
double precision, parameter:: c9=.999d0
common T,n,y
double precision, parameter:: pi=3.14159265358979d0
double precision lamb(T),delta(n),c(n),phi0(n),phi1(n),phi2(n),&
                &alpha0,alpha1,alpha2,eta,si,b(n),llik,sigmat(n,n),gamma(n),gammat(n,n),&
				&cc(n,n),epst(1:T,1:n),isigmat(n,n),igammat(n,n)&
				&,fttm,wttm,nufun,gamfun,besrat,bb,cfun,lt(n)&
				&,sigmastar(n,n),isigmastar(n,n),a,bbp(n,n),alpha,qfun&
				&,besr,besrp,besrm,d

call exportvec(th,"thint.txt",15)

delta=th(1:n)
c(1)=1.0d0
c(2:n)=th(n+1:2*n-1)
alpha0=th(2*n)
phi0=vassign(n,th(2*n+1))
alpha1=c9*(dsin(th(2*n+2))**2.0d0)
alpha2=(c9-alpha1)*(dsin(th(2*n+3))**2.0d0)

forall(i=1:n)
phi1(i)=c9*(dsin(th(2*n+4))**2.0d0)
end forall
forall(i=1:n)
phi2(i)=(c9-phi1(i))*(dsin(th(2*n+5))**2.0d0)
end forall


if (th(2*n+6)>0.0d0) then
	eta=1.01d-3+th(2*n+6)
else
	eta=-1.01d-3+th(2*n+6)
end if

si=th(2*n+7)
b=th(2*n+8:3*n+7)


nufun=-1.0d0/(2.0d0*eta)
gamfun=(1.0d0-si)/si

besrat=besselr(nufun,gamfun)/gamfun



forall(i=1:n,j=1:n)
	cc(i,j)=c(i)*c(j)
	bbp(i,j)=b(i)*b(j)
end forall

forall(i=1:T,j=1:n)
	epst(i,j)=y(i,j)-delta(j)
end forall

!!
gamma=phi0/(1.0d0-phi1-phi2)
gammat=diag(gamma)
igammat=diag(1.0d0/gamma)

lamb(1)=alpha0/(1.0d0-alpha1-alpha2)
sigmat=(cc*lamb(1))+gammat
isigmat=igammat-lamb(1)*(((igammat.x.cc).x.igammat)&
     &/(lamb(1)*dot_product(c,matmul(igammat,c))+1.0d0))
!!
bb=dot_product(b,matmul(sigmat,b))

cfun=ghcfun1(bb,eta,si)
lt=epst(1,:)+cfun*matmul(sigmat,b)

if (bb>1.0d-8) then
    isigmastar=isigmat-((cfun-1.0d0)/(cfun*bb))*bbp
    sigmastar=sigmat+((cfun-1.0d0)/bb)*((sigmat.x.bbp).x.sigmat)
else
    a=besseld(nufun+1.0d0,gamfun)-1.0d0
    isigmastar=isigmat-((-a+2.0d0*a*a*bb*bb-5.0d0*(a**3.0d0)*(bb**2.0d0))/cfun)*bbp
    sigmastar=sigmat+(-a+2.0d0*(a**2.0d0)*(bb**2.0d0)-5.0d0*(a**3.0d0)*(bb**2.0d0))&
	         &*((sigmat.x.bbp).x.sigmat)
end if

qfun=dsqrt(1.0d0+besrat*dot_product(lt,matmul(isigmastar,lt)))

alpha=dsqrt((cfun*bb/besrat)+gamfun*gamfun)

!!

llik=nufun*dlog(gamfun)+.5d0*n*dlog(besrat)-.5d0*n*dlog(2.0d0*pi)&
     &-2.0d0*(nufun-.5d0*n)*dlog(alpha)-robustlbesk(nufun,gamfun)-.5*dlog(det(sigmastar))&
	 &+(nufun-.5d0*n)*dlog(alpha*qfun)+robustlbesk(nufun-.5d0*n,alpha*qfun)+dot_product(b,lt)


do i=2,T,1
	wttm=lamb(i-1)/(1.0d0+lamb(i-1)*dot_product(c,c/gamma))
	fttm=wttm*dot_product((c/gamma),epst(i-1,:))
	lamb(i)=alpha0&
	       &+alpha1*(((fttm)**2.0d0)+wttm)+alpha2*lamb(i-1)


	gamma=phi0+phi1*(((epst(i-1,:)-c*fttm)**2.0d0)+(c*c)*wttm)+phi2*gamma

	gammat=diag(gamma)

	igammat=diag(1.0d0/gamma)

	sigmat=(cc*lamb(i))+gammat
	isigmat=igammat-lamb(i)*(((igammat.x.cc).x.igammat)&
	     &/(lamb(i)*dot_product(c,matmul(igammat,c))+1.0d0))

	!!
	
	bb=dot_product(b,matmul(sigmat,b))
	cfun=ghcfun1(bb,eta,si)
	lt=epst(i,:)+cfun*matmul(sigmat,b)

	if (bb>1.0d-8) then
	    isigmastar=isigmat-((cfun-1.0d0)/(cfun*bb))*bbp
	    sigmastar=sigmat+((cfun-1.0d0)/bb)*((sigmat.x.bbp).x.sigmat)
	else
	    a=besseld(nufun+1.0d0,gamfun)-1.0d0
	    isigmastar=isigmat-((-a+2.0d0*a*a*bb*bb-5.0d0*(a**3.0d0)*(bb**2.0d0))/cfun)*bbp
	    sigmastar=sigmat+(-a+2.0d0*(a**2.0d0)*(bb**2.0d0)-5.0d0*(a**3.0d0)*(bb**2.0d0))&
		         &*((sigmat.x.bbp).x.sigmat)
	end if

		
	qfun=dsqrt(1.0d0+besrat*dot_product(lt,matmul(isigmastar,lt)))
	alpha=dsqrt((cfun*bb/besrat)+gamfun*gamfun)

	!!
	llik=llik+nufun*dlog(gamfun)+.5d0*n*dlog(besrat)-.5d0*n*dlog(2.0d0*pi)&
	     &-2.0d0*(nufun-.5d0*n)*dlog(alpha)-robustlbesk(nufun,gamfun)-.5*dlog(det(sigmastar))&	
		 &+(nufun-.5d0*n)*dlog(alpha*qfun)+robustlbesk(nufun-.5d0*n,alpha*qfun)+dot_product(b,lt)

end do
likresm=-llik
!print*, likresm
end subroutine ghllik


!!!!!!!!!!!!!!!!
subroutine ghscr(m,th,grad)
!lambda=1
use linear_operators
use matrixfuns
use datahadler
use bessels
use dbessels
use ghs
integer, intent(in):: m
double precision, intent(in):: th(m)
double precision, intent(out):: grad(m)
integer, parameter:: fil=1000,col=3
integer T,n,i,j,ii,jj
double precision y(fil,col)
double precision, parameter:: c9=.999d0
common T,n,y
double precision lamb(T),delta(n),c(n),phi0(n),phi1(n),phi2(n),gamma(n),&
                &sigmat(n,n),gammat(n,n),alpha0,alpha1,alpha2,eta,si,b(n)&
				&,cc(n,n),epst(1:T,1:n),isigmat(n,n),igammat(n,n)
double precision ddelta(m-col-2,n),dc(m-col-2,n),dgamma(m-col-2,n),ent(n,n*n)&
               &,dmut(m-col-2,n),dalpha0(m-col-2),dalpha1(m-col-2),dalpha2(m-col-2)&
			   &,dlambda(m-col-2),ident(n,n),wttm,fttm&
			   &,dfttm(m-col-2),dwttm(m-col-2),matwtt(m-col-2),metn(n)&
			   &,dphi0(m-col-2,n),dphi1(m-col-2,n),dphi2(m-col-2,n),nufun,gamfun,besrat,bb,cfun,lt(n)&
			   &,isigmastar(n,n),a,bbp(n,n)&
			   &,dlgKsi,besr,dlgRsi,besrp,besrm,d,dDsi,deriv,deriv1,dlgKeta,dlgReta,dDeta,epsti(n)
double precision p1,p2,ff,gg,rt,coefcfun,coef2,ltl,dcfuneta,dcfunsi,&
&elogsi,sigb(n),isiglt(n),isigltdub(n,n),ghgradi(m)&
&,lami,dv1(m-col-2),dv2(m-col-2),dv3(m-col-2),dv4(m-col-2),dvstar(m-col-2)&
&,dp1(m-col-2),dp2(m-col-2),dlgKeta2,dlgReta2,dDeta2

!call exportvec(th,"thin2.txt",15)

ent=matent(n)
ident=eye(n)

delta=th(1:n)
c(1)=1.0d0
c(2:n)=th(n+1:2*n-1)
alpha0=th(2*n)
phi0=vassign(n,th(2*n+1))
alpha1=c9*(dsin(th(2*n+2))**2.0d0)
alpha2=(c9-alpha1)*(dsin(th(2*n+3))**2.0d0)

forall(i=1:n)
phi1(i)=c9*(dsin(th(2*n+4))**2.0d0)
end forall
forall(i=1:n)
phi2(i)=(c9-phi1(i))*(dsin(th(2*n+5))**2.0d0)
end forall


if (th(2*n+6)>0.0d0) then
	eta=1.01d-3+th(2*n+6)
else
	eta=-1.01d-3+th(2*n+6)
end if
si=th(2*n+7)
b=th(2*n+8:3*n+7)


nufun=-1.0d0/(2.0d0*eta)
gamfun=(1.0d0-si)/si
besrat=besselr(nufun,gamfun)/gamfun


dlgKsi=0.5d0*(1.0d0/si**2.0d0)*((1.0d0/besselr(nufun-1.0d0,gamfun))+besselr(nufun,gamfun))

besr=besselr(nufun,gamfun)
dlgRsi=-(1.0d0/si**2.0d0)*(besr-((2.0d0*nufun+1.0d0)/gamfun)-(1.0d0/besr))

besrp=besselr(nufun+1.0d0,gamfun)
besrm=besselr(-nufun-1.0d0,gamfun)
d=besseld(nufun+1.0d0,gamfun)
dDsi=d*(besrp-(1.0d0/besrp)+besrm-(1.0d0/besrm)-(2.0d0/gamfun))*(-1.0d0/si**2.0d0)

a=besseld(nufun+1.0d0,gamfun)-1.0d0
deriv=vdlogk(nufun,gamfun,1.0d-8)
deriv1=vdlogk(nufun+1.0d0,gamfun,1.0d-8)

dlgKeta=deriv/(2.0d0*eta**2.0d0)
dlgReta=(deriv1-deriv)/(2.0d0*eta**2.0d0)
dDeta=besseld(nufun+1.0d0,gamfun)*(vdlogk(nufun+2.0d0,gamfun,1.0d-8)+deriv-2.0d0*deriv1)/(2.0d0*eta**2.0d0)


forall(i=1:m-col-2,j=1:n)
	ddelta(i,j)=0.0d0
	dc(i,j)=0.0d0
	dgamma(i,j)=0.0d0
	dphi0(i,j)=0.0d0
	dphi1(i,j)=0.0d0
	dphi2(i,j)=0.0d0
	dmut(i,j)=0.0d0
end forall
forall(i=1:n)
	ddelta(i,i)=1.0d0
	dphi0(2*n+1,i)=1.0d0
	dphi1(2*n+4,i)=c9*2.0d0*dsin(th(2*n+4))*dcos(th(2*n+4))
	dphi2(2*n+4,i)=-(c9*2.0d0*dsin(th(2*n+4))*dcos(th(2*n+4)))*(dsin(th(2*n+5))**2.0d0)
	dphi2(2*n+5,i)=(c9-phi1(i))*2.0d0*dsin(th(2*n+5))*dcos(th(2*n+5))
end forall
forall(i=2:n)
	dc(n+i-1,i)=1.0d0
end forall

forall(i=1:m-col-2)
	dalpha0(i)=0.0d0
	dalpha1(i)=0.0d0
	dlambda(i)=0.0d0
end forall
dalpha2=dalpha1
dalpha0(2*n)=1.0d0
dalpha1(2*n+2)=c9*2.0d0*dsin(th(2*n+2))*dcos(th(2*n+2))
dalpha2(2*n+2)=-dalpha1(2*n+2)*(dsin(th(2*n+3))**2.0d0)
dalpha2(2*n+3)=(c9-alpha1)*2.0d0*dsin(th(2*n+3))*dcos(th(2*n+3))

forall(i=1:n,j=1:n)
	cc(i,j)=c(i)*c(j)
	bbp(i,j)=b(i)*b(j)
end forall

forall(i=1:n)
	dmut(i,i)=1.0d0
end forall

forall(i=1:T,j=1:n)
	epst(i,j)=y(i,j)-delta(j)
end forall

gamma=phi0/(1.0d0-phi1-phi2)

gammat=diag(gamma)
igammat=diag(1.0d0/gamma)
forall (i=1:m-col-2,j=1:n)
	dgamma(i,j)=((1.0d0-phi1(j)-phi2(j))*dphi0(i,j)-phi0(j)*(-dphi1(i,j)-dphi2(i,j)))&
	           &/(1.0d0-phi1(j)-phi2(j))**2.0d0
end forall
dlambda=((1.0d0-alpha1-alpha2)*dalpha0-alpha0*(-dalpha1-dalpha2))/(1.0d0-alpha1-alpha2)**2.0d0

lamb(1)=alpha0/(1.0d0-alpha1-alpha2)
sigmat=lamb(1)*cc+gammat
isigmat=igammat-(((igammat.x.cc).x.igammat)&
	      &/(dot_product(c,matmul(igammat,c))+(1.0d0/lamb(1))))


i=1
epsti=epst(1,:)
lami=lamb(1)

call gradi()

grad=ghgradi


!t>1
do i=2,T,1
	wttm=lamb(i-1)/(1.0d0+lamb(i-1)*dot_product(c,c/gamma))
	fttm=wttm*dot_product((c/gamma),epst(i-1,:))
	lamb(i)=alpha0+alpha1*((fttm**2.0d0)+wttm)+alpha2*lamb(i-1)

	forall(ii=1:n)
	    metn(ii)=dot_product(igammat(ii,:),c)*dot_product(igammat(:,ii),epst(i-1,:))
	end forall
	matwtt=matmul(matmul(dgamma,ent),vec2((igammat.x.cc).x.igammat))

	dwttm=-(wttm*wttm)*(-(lamb(i-1)**(-2.0))*dlambda+2.0d0*matmul(dc,matmul(igammat,c))&
	      &-matwtt)
	
	dfttm=dwttm*dot_product(c,matmul(igammat,epst(i-1,:)))&
	     &+wttm*matmul(dc,matmul(igammat,epst(i-1,:)))&
		 &-wttm*matmul(dgamma,metn)-wttm*matmul(dmut,matmul(igammat,c))


	forall (ii=1:m-col-2,j=1:n)
		dgamma(ii,j)=dphi0(ii,j)+dphi1(ii,j)*(((epst(i-1,j)-c(j)*fttm)**2.0d0)&
		                                                                   &+(c(j)*c(j))*wttm)&
		           &+dphi2(ii,j)*gamma(j)&
				   &+phi1(j)*2.0d0*(epst(i-1,j)-c(j)*fttm)*&
				   &(-dmut(ii,j)-dc(ii,j)*fttm-c(j)*dfttm(ii))&
				   &+phi1(j)*2.0d0*c(j)*dc(ii,j)*wttm+phi1(j)*c(j)*c(j)*dwttm(ii)+phi2(j)*dgamma(ii,j)
	end forall


	gamma=phi0+phi1*(((epst(i-1,:)-c*fttm)**2.0d0)+(c*c)*wttm)+phi2*gamma

	gammat=diag(gamma)
	igammat=diag(1.0d0/gamma)


	sigmat=lamb(i)*cc+gammat
	isigmat=igammat-(((igammat.x.cc).x.igammat)&
	      &/(dot_product(c,matmul(igammat,c))+(1.0d0/lamb(i))))

	dlambda=dalpha0+dalpha1*((fttm*fttm)+wttm)+dalpha2*(lamb(i-1))&
	      &+alpha1*(2.0d0*fttm*dfttm+dwttm)+alpha2*dlambda

		
	lami=lamb(i)
	epsti=epst(i,:)
	call gradi()

	grad=grad+ghgradi

end do

grad=-grad
contains
subroutine gradi()
use bessels
use linear_operators
use matrixfuns

bb=dot_product(b,matmul(sigmat,b))
cfun=ghcfun1(bb,eta,si)
lt=epsti+cfun*matmul(sigmat,b)
rt=dsqrt(1.0d0+4.0d0*a*bb)

if (bb > 1.0d-8) then
    coefcfun=(cfun-1.0d0)/(bb)
    coef2=coefcfun*((1.0d0/rt)-1.0d0)/bb
else
    coefcfun=-a+2.0d0*a**2.0d0*bb-5.0d0*a**3.0d0*bb**2.0d0+14.0d0*a**4.0d0*bb**3.0d0
    coef2=coefcfun*(-2.0d0*a+6.0d0*a**2.0d0*bb-20.0d0*a**3.0d0*bb**2.0d0+70.0d0*a**4.0d0*bb**3.0d0)
end if
isigmastar=isigmat-(coefcfun/cfun)*bbp

ltl=dot_product(lt,matmul(isigmastar,lt))

p1=dsqrt((bb/besrat)*cfun+gamfun**2.0d0)
p2=dsqrt(besrat*ltl+1.0d0)


ff=besrat*(p1/p2)*besselr(.5d0*n-nufun,p1*p2)
gg=p2/(p1*besselr(.5d0*n-nufun-1.0d0,p1*p2)*besrat)


!! th
isiglt=matmul(isigmastar,lt)
forall(ii=1:n,jj=1:n)
	isigltdub(ii,jj)=isiglt(ii)*isiglt(jj)
end forall



dv1=matmul(dgamma,vecd(isigmat))+dlambda*dot_product(c,matmul(isigmat,c))&
  &+2.0d0*lami*matmul(dc.x.isigmat,c)
dv2=matmul(dgamma,b*b)+dlambda*dot_product(c,b)**2.0d0&
  &+2.0d0*lami*matmul(dc,b)*dot_product(c,b)

dv3=matmul(dgamma,vecd(isigltdub))+dlambda*dot_product(c,matmul(isigltdub,c))&
  &+2.0d0*lami*matmul(dc.x.isigltdub,c)

dv4=2.0d0*matmul(dgamma,isiglt*b)+2.0d0*dlambda*dot_product(c,isiglt)*dot_product(b,c)&
&+2.0d0*lami*(dot_product(b,c)*matmul(dc,isiglt)+dot_product(isiglt,c)*matmul(dc,b))

dvstar=dv3+(coefcfun*dot_product(b,lt)/cfun)*dv4&
&+(coef2*(dot_product(b,lt)/cfun)**2.0d0)*dv2


dp1=-matmul(dmut,b)+cfun*(matmul(dgamma,b*b)+dlambda*dot_product(c,b)**2.0d0&
   &+2.0d0*lami*dot_product(c,b)*matmul(dc,b))+(coefcfun*bb/rt)*dv2

dp2=-matmul(dmut,isiglt)+cfun*(matmul(dgamma,isiglt*b)&
&+dlambda*dot_product(c,b)*dot_product(c,isiglt)&
&+lami*matmul(dc,dot_product(c,isiglt)*b+dot_product(c,b)*isiglt))&
&+(coefcfun*dot_product(isiglt,matmul(sigmat,b))/rt)*dv2


ghgradi(1:2*n+5)=-.5d0*dv1-ff*dp2&
     &-.5d0*(coefcfun/(cfun*rt))*dv2+dp1&
    &+.5d0*ff*dvstar-.5d0*(gg/rt)*dv2

!! eta
dcfuneta=((cfun-1.0d0)/(a*rt))*dDeta
elogsi=dlog(p1/p2)+vdlogk(.5d0*n-nufun,p1*p2,1.0d-8)


ghgradi(2*n+6)=.5d0*n*dlgReta+(bb-(.5d0/cfun))*dcfuneta+(dlog(gamfun)/(2.0d0*eta**2.0d0))&
       &-dlgKeta-(1.0d0/(2.0d0*eta**2.0d0))*elogsi &
       &-.5d0*ff*(dlgReta*ltl+dcfuneta*bb&
	   &-(dot_product(b,epsti)**2.0d0*coefcfun*dDeta/(cfun**2.0d0*a*rt)))&
       &-.5d0*bb*gg*(dcfuneta-cfun*dlgReta)



!! si
dcfunsi=((cfun-1.0d0)/(a*rt))*dDsi
ghgradi(2*n+7)=.5d0*n*dlgRsi+(n/(2.0d0*si*(1.0d0-si)))&
     &+(bb-(.5d0/cfun))*dcfunsi+(1.0d0/(2.0d0*eta*si*(1.0d0-si)))-dlgKsi&
     &-.5d0*ff*((dlgRsi+(1.0d0/(si*(1.0d0-si))))*ltl&
	 &+bb*dcfunsi-(coefcfun*dot_product(b,epsti)**2.0d0*dDsi/(cfun**2.0d0*a*rt)))&
     &-.5d0*bb*gg*(-(cfun/(si*(1.0d0-si)))+dcfunsi-cfun*dlgRsi)&
	 &+gg*(besr/si**2.0d0)

!! b
sigb=matmul(sigmat,b)
ghgradi(2*n+8:3*n+7)=-(coefcfun/(cfun*rt))*sigb-ff*cfun*lt+epsti &
     &+ff*coefcfun*dot_product(b,lt)*((coefcfun*dot_product(b,lt)/(cfun**2.0d0*rt))*sigb&
	 &+(lt/cfun)-(sigb/rt))&
     &+((2.0d0-gg)/rt)*sigb

end subroutine gradi
end subroutine ghscr
end module ghlliks